﻿#include "OiPCH.hpp"
#include <QTimer>
#include "View/OiPixmap.hpp"
#include "Tools/OiPlyTool.hpp"
#include <omp.h>
#include "View/OiMeasureTool.hpp"
#include "Math/OiOptimize.hpp"

namespace Oi { namespace PLScannerCore {

    class PLScannerWorkerImpl;

    namespace Environment
    {
        bool _startTask;
        bool _newParams;
    }

    inline void normalizePose(Math::Matrix33f& mat)
    {
        cv::Mat A = (cv::Mat_<double>(2, 2) << mat.elementAt(0, 0), mat.elementAt(0, 1), mat.elementAt(1, 0), mat.elementAt(1, 1));
        cv::Mat s, u, vt;
        cv::SVD::compute(A, s, u, vt);

        double s00, s11;
        if (s.at<double>(0, 0) > 0)
        {
            s00 = 1;
        }
        else
        {
            s00 = -1;
        }

        if (s.at<double>(1, 0) > 0)
        {
            s11 = 1;
        }
        else
        {
            s11 = -1;
        }

        cv::Mat Smat = (cv::Mat_<double>(2, 2) << s00, 0, 0, s11);
        cv::Mat rotationMat = u * Smat * vt;

        mat.elementAt(0, 0) = rotationMat.at<double>(0, 0);
        mat.elementAt(0, 1) = rotationMat.at<double>(0, 1);
        mat.elementAt(1, 0) = rotationMat.at<double>(1, 0);
        mat.elementAt(1, 1) = rotationMat.at<double>(1, 1);
    }

    inline Math::Matrix33f translationMatrix(float Tx, float Ty)
    {
        Math::Matrix33f result;
        result.setIdentity();
        result.elementAt(0, 2) = Tx;
        result.elementAt(1, 2) = Ty;
        return result;
    }

    inline Math::Matrix33f rotationMatrix(float angleInRad)
    {
        Math::Matrix33f result;
        result.setIdentity();
        result.elementAt(0, 0) = std::cos(angleInRad);
        result.elementAt(0, 1) = -std::sin(angleInRad);
        result.elementAt(1, 0) = std::sin(angleInRad);
        result.elementAt(1, 1) = std::cos(angleInRad);
        return result;
    }

    inline Math::Matrix33f mirrorMatrix()
    {
        Math::Matrix33f result;
        result.setIdentity();
        result.elementAt(1, 1) = -1;
        return result;
    }

    inline float x(const Math::Point2f& point)
    {
        return point.x();
    }

    inline float y(const Math::Point2f& point)
    {
        return point.y();
    }
    
    inline float x(const QPointF& point)
    {
        return point.x();
    }

    inline float y(const QPointF& point)
    {
        return point.y();
    }

    template <class Pt>
    void transformPoint(Pt& point, Math::Matrix33f& transformation)
    {
        point = Pt(x(point) * transformation.elementAt(0, 0) + y(point) * transformation.elementAt(0, 1) + transformation.elementAt(0, 2), x(point) * transformation.elementAt(1, 0) + y(point) * transformation.elementAt(1, 1) + transformation.elementAt(1, 2));
    }

    template <class PtCloud>
    void ICPTemplateMultiThreaded(PtCloud& fullScene, Core::KDTreePtr model, Math::Matrix33f& initialCondition, float error, int iterationLimit, float contactDistance, int subsample)
    {
        PtCloud scene;
        scene.reserve(fullScene.size() / subsample);
        for (int i = 0; i < fullScene.size(); i = i + subsample)
        {
            scene.push_back(fullScene[i]);
        }

        int iterationCount = 0;

        float errorSq = error * error;
        float contactDistanceSq = contactDistance * contactDistance;

        for (int i = 0; i < scene.size(); i++)
        {
            //if (!std::isnan(x(scene[i])))
            {
                transformPoint(scene[i], initialCondition);
            }
        }

        float* buffer = new float[4 * scene.size()];

        while (iterationCount < iterationLimit)
        {
            float modelCOMx = 0;
            float modelCOMy = 0;
            float sceneCOMx = 0;
            float sceneCOMy = 0;

            float globalDistanceSq = std::numeric_limits<float>::lowest();

            int count = 0;

            for (int i = 0; i < 4 * scene.size(); i++)
            {
                buffer[i] = std::numeric_limits<float>::quiet_NaN();;
            }

#pragma omp parallel
            {
                float tempDistanceSq = globalDistanceSq;
#pragma omp for schedule(static) reduction(+:modelCOMx, modelCOMy, sceneCOMx, sceneCOMy, count)
                for (int i = 0; i < scene.size(); i++)
                {
                    //if (std::isnan(x(scene[i]))) continue;
                    int index = i * 4;
                    float nearestDistanceSq = float(contactDistanceSq);
                    Math::Point2f nearestPoint = model->nearestPoint(x(scene[i]), y(scene[i]), nearestDistanceSq);
                    if (nearestDistanceSq >= 0)
                    {
                        buffer[index] = x(scene[i]);
                        buffer[index + 1] = y(scene[i]);
                        buffer[index + 2] = x(nearestPoint);
                        buffer[index + 3] = y(nearestPoint);
                        sceneCOMx = sceneCOMx + x(scene[i]);
                        sceneCOMy = sceneCOMy + y(scene[i]);
                        modelCOMx = modelCOMx + x(nearestPoint);
                        modelCOMy = modelCOMy + y(nearestPoint);
                        count += 1;
                        if (tempDistanceSq < nearestDistanceSq) tempDistanceSq = nearestDistanceSq;
                    }
                    else
                    {
                        //buffer[index] = std::numeric_limits<float>::quiet_NaN();
                        //buffer[index + 1] = std::numeric_limits<float>::quiet_NaN();
                        //buffer[index + 2] = std::numeric_limits<float>::quiet_NaN();
                        //buffer[index + 3] = std::numeric_limits<float>::quiet_NaN();
                    }
                }
#pragma omp critical
                if (globalDistanceSq < tempDistanceSq) globalDistanceSq = tempDistanceSq;
            }


            if (count == 0)
            {
                delete buffer;
                return;
            }

            if (globalDistanceSq <= errorSq)
            {
                delete buffer;
                return;
            }

            modelCOMx = modelCOMx / float(count);
            modelCOMy = modelCOMy / float(count);
            sceneCOMx = sceneCOMx / float(count);
            sceneCOMy = sceneCOMy / float(count);

            float A00 = 0;
            float A01 = 0;
            float A10 = 0;
            float A11 = 0;

#pragma omp parallel for schedule(static) reduction(+:A00, A01, A10, A11)
            for (int i = 0; i < scene.size(); i++)
            {
                int index = i * 4;
                if (!std::isnan(buffer[index]))
                {
                    buffer[index] = buffer[index] - sceneCOMx;//scenePointx
                    buffer[index + 1] = buffer[index + 1] - sceneCOMy;//scenePointy
                    buffer[index + 2] = buffer[index + 2] - modelCOMx;//modelPointx
                    buffer[index + 3] = buffer[index + 3] - modelCOMy;//modelPointy
                    A00 = A00 + buffer[index] * buffer[index + 2];
                    A01 = A01 + buffer[index] * buffer[index + 3];
                    A10 = A10 + buffer[index + 1] * buffer[index + 2];
                    A11 = A11 + buffer[index + 1] * buffer[index + 3];
                }
            }

            cv::Mat A = (cv::Mat_<float>(2, 2) << A00, A01, A10, A11);

            cv::Mat S, U, V;
            cv::SVD::compute(A, S, U, V);
            cv::Mat R = (U * V).t();
            cv::Mat Reflection = (cv::Mat_<float>(2, 2) << 1, 0, 0, -1);
            if (cv::determinant(R) < 0)
                R = R * Reflection;

            Math::Point2f transVector = Math::Point2f(modelCOMx, modelCOMy) - Math::Point2f(R.at<float>(0, 0) * sceneCOMx + R.at<float>(0, 1) * sceneCOMy, R.at<float>(1, 0) * sceneCOMx + R.at<float>(1, 1) * sceneCOMy);

            Math::Matrix33f iterationMat;
            iterationMat.elementAt(0, 0) = R.at<float>(0, 0);
            iterationMat.elementAt(0, 1) = R.at<float>(0, 1);
            iterationMat.elementAt(0, 2) = transVector.x();
            iterationMat.elementAt(1, 0) = R.at<float>(1, 0);
            iterationMat.elementAt(1, 1) = R.at<float>(1, 1);
            iterationMat.elementAt(1, 2) = transVector.y();
            iterationMat.elementAt(2, 0) = 0;
            iterationMat.elementAt(2, 1) = 0;
            iterationMat.elementAt(2, 2) = 1;

            initialCondition = iterationMat * initialCondition;

#pragma omp parallel for schedule(static)
            for (int i = 0; i < scene.size(); i++)
            {
                //if (!std::isnan(x(scene[i])))
                {
                    transformPoint(scene[i], iterationMat);
                }
            }

            iterationCount++;
        }

#pragma omp parallel for schedule(static)
        for (int i = 0; i < fullScene.size(); i++)
        {
            transformPoint(fullScene[i], initialCondition);
        }

        delete buffer;
        return;
    }

    static Math::Matrix33f accurateMatch(const QList<QPointF>& points, Core::KDTreePtr templateTree)
    {
        std::vector<Math::Point2f> fullProfile;

        for (auto& point : points)
        {
            fullProfile.emplace_back(float(point.x()), float(point.y()));
        }

        cv::Point2f designCenter(0, 0);
        cv::Point2f profileCenter(0, 0);

        for (int i = 0; i < templateTree->size(); i++)
        {
            designCenter.x = designCenter.x + templateTree->x(i);
            designCenter.y = designCenter.y + templateTree->y(i);
        }

        for (auto& point : fullProfile)
        {
            profileCenter.x = profileCenter.x + point.x();
            profileCenter.y = profileCenter.y + point.y();
        }

        designCenter = designCenter / float(templateTree->size());
        profileCenter = profileCenter / float(fullProfile.size());

        auto translation = translationMatrix(designCenter.x - profileCenter.x, designCenter.y - profileCenter.y);

        for (auto& point : fullProfile)
        {
            point = translation * point;
        }

        int step = 50;

        std::vector<float> score(step, 0.f);

        auto transToZero = translationMatrix(-designCenter.x, -designCenter.y);
        auto transToZeroInv = translationMatrix(designCenter.x, designCenter.y);
        auto mirror = mirrorMatrix();

        if (Core::OiSettings()->value("testMode", false).value<bool>())
        {
            mirror = Math::Matrix33f::identity();
        }

#pragma omp parallel for schedule(static)
        for (int k = 0; k < step; k++)
        {
            std::vector<Math::Point2f> tempProfile = fullProfile;
            auto homRotation = transToZeroInv * mirror * rotationMatrix(2.0 * CV_PI * float(k) / float(step)) * transToZero;
            Core::Model2D::ICP(tempProfile, templateTree, homRotation, 0, 25, 9999, false);
            for (auto& point : tempProfile)
            {
                float minDistanceSq = std::numeric_limits<float>::max();
                templateTree->nearestPoint(point, minDistanceSq);
                score[k] = score[k] + minDistanceSq;
            }
        }

        float minScore = std::numeric_limits<float>::max();
        int min;

        for (int k = 0; k < step; k++)
        {
            if (score[k] < minScore)
            {
                minScore = score[k];
                min = k;
            }
        }

        // 历史遗留问题，强制镜像
        Math::Matrix33f bestRotation = transToZeroInv * mirror * rotationMatrix(2.0 * CV_PI * float(min) / float(step)) * transToZero;

        Core::Model2D::ICP(fullProfile, templateTree, bestRotation, 0.1, 50, std::numeric_limits<float>::max());

        Math::Matrix33f currentPose = bestRotation * translation;

        return currentPose;
    }

    class TaskQueue
    {
    public:
        TaskQueue()
        {
            _threadPool = QThreadPool::globalInstance();
        }
        ~TaskQueue()
        {
        }

    public:
        void setQueueSizeLimit(int);
        void enqueue(QRunnable*);
        void dequeue();
        void clearQueue();

    private:
        QThreadPool* _threadPool;
        QQueue<QRunnable*> _queue;
        bool _threadPoolReady = true;
        int _queueLimit = 5;
        QString _warning = QObject::tr("掉帧!");
    };

    void TaskQueue::setQueueSizeLimit(int queueLimit)
    {
        _queueLimit = queueLimit;
    }

    void TaskQueue::enqueue(QRunnable* task)
    {
        if (_threadPoolReady)
        {
            _threadPoolReady = false;
            _threadPool->start(task);
        }
        else
        {
            if (_queue.size() <= _queueLimit)
            {
                _queue.enqueue(task);
            }
            else
            {
                OiWarning() << _warning;
                delete task;
                clearQueue();
            }
        }
    }

    void TaskQueue::clearQueue()
    {
        for (auto& task : _queue)
        {
            delete task;
        }
        _queue.clear();
        _threadPoolReady = true;
    }

    void TaskQueue::dequeue()
    {
        if (!_queue.empty())
        {
            _threadPool->start(_queue.dequeue());
        }
        else
        {
            _threadPoolReady = true;
        }
    }

    namespace WorkResult
    {
        Result::Result()
        {
        }

        Result::Result(const Result& p)
        {
            *this = p;
        }

        Result& Result::operator=(const Result& p)
        {
            result = p.result;
            profile1 = p.profile1;
            profile2 = p.profile2;
            data = p.data;
            params = p.params;
            return *this;
        }

        void Result::caculateResult(Result & result, bool dir, void* data)
        {
            PlyTool tool;
            tool.calResult(result, dir);

            //PLScannerWorkerImpl* impl = reinterpret_cast<PLScannerWorkerImpl*>(data);
        }

        void Result::saveResultToDataBase(const Result & result)
        {
//             DataRow row;
//             row.datetime = QDateTime::currentDateTime();
//             row.result = result.result;
//             row.formula = result.params.specName;
//             row.insResult = result.data;
//             row.profile = result.profile;
// 
//             if(Database::instance()->insert(row))
//             {
//             }
        }

        void Result::saveResultToFile(const Result & result, const QString path)
        {
            if(QFileInfo::exists(path))
            {
                QFile::remove(path);
            }

            Core::ZipWriter writer(path);

            if(writer.result() != 0)
            {
                return;
            }

//             QByteArray info;
//             QDataStream sInfo(&info, QIODevice::WriteOnly);
//             sInfo << result.result << result.data;

            QByteArray profile;
            QDataStream sProfile(&profile, QIODevice::WriteOnly);
            sProfile << result.profile1;

//            writer.addFile("info.dat", info);
            writer.addFile("profile.dat", profile);

            QString tempFilePath;
            QTemporaryDir dir;
            if(dir.isValid())
            {
                tempFilePath = dir.path() + "/" + QUuid::createUuid().toString();
            }
            else
            {
                tempFilePath = Core::OiGetPermanentDir(TempDir) + "/" + QUuid::createUuid().toString();
            }

            if(ProductParams::saveToLocal(result.params, tempFilePath))
            {
                QFile tempFile(tempFilePath);
                if(tempFile.open(QIODevice::ReadOnly))
                {
                    writer.addFile("params.dat", tempFile.readAll());
                }
            }       
        }

        void Result::loadResultFromDataBase(Result & result, const int id)
        {
//             DataRow row;
//             if(Database::instance()->query(row, id))
//             {
//                 result.result = row.result;
//                 ProductParams::loadFromLocal(result.params, row.formula);
//                 result.data = row.insResult;
//                 result.profile = row.profile;
//             }
        }

        void Result::loadResultFromFile(Result & result, const QString path)
        {
            if(!QFileInfo::exists(path))
            {
                return;
            }

            Core::ZipReader reader(path);

            if(reader.result() != 0)
            {
                return;
            }

//             if(reader.hasFile("info.dat"))
//             {
//                 auto idata = reader.fileData("info.dat");
//                 QDataStream stream(&idata, QIODevice::ReadOnly);
// 
//                 if(!stream.atEnd())
//                 {
//                     stream >> result.result >> result.data;
//                 }
//             }

            if(reader.hasFile("profile.dat"))
            {
                auto pdata = reader.fileData("profile.dat");
                QDataStream stream(&pdata, QIODevice::ReadOnly);

                if(!stream.atEnd())
                {
                    stream >> result.profile1;
                }
            }

            if (reader.hasFile("params.dat"))
            {
                QString tempFilePath;
                QTemporaryDir dir;
                if(dir.isValid())
                {
                    tempFilePath = dir.path() + "/" + QUuid::createUuid().toString() + ".pmf";
                }
                else
                {
                    tempFilePath = Core::OiGetPermanentDir(TempDir) + "/" + QUuid::createUuid().toString() + ".pmf";
                }

                QFile tempFile(tempFilePath);
                if(tempFile.open(QIODevice::WriteOnly))
                {
                    tempFile.write(reader.fileData("params.dat"));
                    tempFile.close();
                }

                ProductParams::loadFromLocal(result.params, QString(), tempFilePath);
            }
        }
    }

    class WorkLoopModeTaskImpl : public Impl<WorkLoopModeTask>
    {
    public:
        PLScannerWorkerImpl* _impl = nullptr;        
    };

    class ProfileMeasurementWorker : public QRunnable
    {
    public:
        QObject* _self;
        QMap<QString, Core::RangeMapPtr> _rangeMaps;
        std::vector<cv::Point2f> _design;
        Core::KDTreePtr _designTree;
        Math::Matrix33f _currentPose;
        QDateTime _timestamp;
        int extrusionMode = 0;
        bool templFlag = false;
        MeasureTool* msrTool = nullptr;

    public:
        ProfileMeasurementWorker(QObject* self, const QMap<QString, Core::RangeMapPtr>& rangeMaps, const QDateTime& timestamp, const std::vector<cv::Point2f>& design, Core::KDTreePtr designTree, Math::Matrix33f currentPose)
        {
            _self = self;
            _design = design;
            _designTree = designTree;
            _currentPose = currentPose;
            _rangeMaps = rangeMaps;
            _timestamp = timestamp;
            extrusionMode = Core::OiSettings()->value("CaptureType", 1).toInt() == 1 ? 0 : 1;

        }
        ~ProfileMeasurementWorker()
        {

        }

    public:
        void setTempl(bool flag)
        {
            templFlag = flag;
        }

        void setMsrTool(MeasureTool* tool)
        {
            msrTool = tool;
        }

        void run() override
        {
            Base::setThreadName(0, "ProfileMeasurement");

            double smoothingRadiusSq = LT360X::instance()->smoothingRadius() * LT360X::instance()->smoothingRadius();

            QList<QPointF> profile;
            LT360X::instance()->rangeMapsToPoints(_rangeMaps, profile, false);

            if (Core::OiSettings()->value("testMode", false).value<bool>())
            {
                profile.clear();
                for (auto& arr : _design)
                {
                    profile.push_back(QPointF(arr.x, arr.y));
                }
            }

            if (profile.size() < 50)
            {
                OiWarning() << "Profile is empty";
                QMetaObject::invokeMethod(
                    _self,
                    "measureDone",
                    Qt::QueuedConnection);
                return;
            }
            Math::smoothProfile(profile, LT360X::instance()->smoothingRadius());
            OiProfiles filteredProfile = Math::profileParser(profile, LT360X::instance()->connectionThreshold(), LT360X::instance()->linkingThreshold(), LT360X::instance()->noiseThreshold());
            profile.clear();
            for (auto& contour : filteredProfile)
            {
                for (auto& point : contour)
                {
                    profile.append(point);
                }
            }

            if (profile.size() < 50)
            {
                OiWarning() << "Parsed profile is empty";
                QMetaObject::invokeMethod(
                    _self,
                    "measureDone",
                    Qt::QueuedConnection);
                return;
            }

            if (templFlag)
            {
                _currentPose = accurateMatch(profile, _designTree);

                for (int i = 0; i < profile.size(); i++)
                {
                    transformPoint(profile[i], _currentPose);
                }

                if (extrusionMode == 1)
                {
                    double leftX = std::numeric_limits<double>::max();
                    double rightX = std::numeric_limits<double>::lowest();
                    for (auto& point : profile)
                    {
                        if (point.x() < leftX) leftX = point.x();
                        if (point.x() > rightX) rightX = point.x();
                    }

                    double mid = (leftX + rightX) * 0.5;

                    QList<QPointF> leftProfile;
                    QList<QPointF> rightProfile;

                    for (auto& point : profile)
                    {
                        if (point.x() < mid)
                        {
                            leftProfile << point;
                        }
                        else
                        {
                            rightProfile << point;
                        }
                    }

                    Math::Matrix33f leftPose;
                    leftPose.setIdentity();
                    Math::Matrix33f rightPose;
                    rightPose.setIdentity();

                    if (leftProfile.size() > 50 && rightProfile.size() > 50)
                    {
                        Core::Model2D::ICP(leftProfile, _designTree, leftPose, 0, 25, 3, false);
                        Core::Model2D::ICP(rightProfile, _designTree, rightPose, 0, 25, 3, false);
                    }

                    profile.clear();

                    for (auto& point : leftProfile)
                    {
                        profile << point;
                    }

                    for (auto& point : rightProfile)
                    {
                        profile << point;
                    }
                }
                std::vector<cv::Point2f> orderlyProfile;
                try
                {
                    for (auto& p : profile)
                    {
                        orderlyProfile.push_back(cv::Point2f(p.x(), p.y()));
                    }
                    orderlyProfile = Math::orderDiscreteProfile(orderlyProfile, 16);
                    profile.clear();
                    for (auto& p : orderlyProfile)
                    {
                        profile.push_back(QPointF(p.x, p.y));
                    }
                }
                catch (...)
                {
                    profile.clear();
                }

                if (profile.size() < 50)
                {
                    OiWarning() << "Parsed profile is empty";
                    QMetaObject::invokeMethod(
                        _self,
                        "measureDone",
                        Qt::QueuedConnection);
                    return;
                }
            }

            std::vector<QPointF> msrRes;
            if (templFlag && msrTool)
            {
                msrRes = msrTool->measure(profile);
            }

            QMetaObject::invokeMethod(
                _self,
                "measureDone",
                Qt::QueuedConnection,
                Q_ARG(const LTCameraFrame&, _rangeMaps),
                Q_ARG(const QDateTime&, _timestamp),
                Q_ARG(const QList<QPointF>&, profile),
                Q_ARG(const std::vector<QPointF>&, msrRes),
                Q_ARG(const Math::Matrix33f&, _currentPose));
        }
    };

    class PLScannerWorkerImpl : public Impl<PLScannerWorker>
    {
    public:
        PLScannerWorkerImpl() 
        {
        }

    public:
        void init();
        void exit();
        void start();
        void stop();
        static void snapRefresh(const LTCameraFrame&, const QDateTime&, void*);
        static void refresh(const LTCameraFrame&, const QDateTime&, void*);

    public:
        QTimer _timerProduct;
        int _timeCalib;
        int _heartbeat = 0;
        QTimer _updateChartTimer;
    public:
        WorkLoopModeTask* _loopTaskThread;

    public:
        ProductParams _params;
        MeasureTool* _measureTool = nullptr;

    public:
        WorkResult::Result _result;
        double _fixedValue;

    public:
        QMutex _mutex;

    public:
        QFuture<void> _future;
        bool _isRunning = false;
        TaskQueue _profileMeasurementTasks;
        TaskQueue _reconstruction3DTasks;
    public:
        int _workRet;
        int _firstProfileSize = 0;
        int _accumulatedFrames = 0;

        //data
        QList<QPointF> _currentProfile;
        Math::Matrix33f _currentPose;
        Math::Matrix33f _initialPose;
        std::vector<QPointF> _curMsrRes;

        //parameters
        std::vector<cv::Point2f> _template;
        Core::KDTreePtr _templateTree;

        //int _detectMode = 0;
    public:
        void wideCalib(OiProfile& ps, double start = 0);

        double wideCalib(double pos);
    };

    void PLScannerWorkerImpl::wideCalib(OiProfile& ps, double start)
    {
        if (!PLScanner::instance()->wideCalibEnable)
        {
            return;
        }
        auto wcp = PLScanner::instance()->wideCalib_P;

        int i = 0, j = 0;
        for (; i < ps.size(); i++)
        {
            while (j < wcp.size())
            {
                 if (ps[i].y() > 20 || ps[i].y() < 0.02)
                 {
                     if (i< ps.size()-1 && i > 0)
                     {
                         if (ps[i + 1].y() >0.01 && ps[i + 1].y() < 10 )
                         {
                             ps[i].setY(ps[i - 1].y());
                         }
                         else if (i+2 < ps.size()-1 &&  ps[i + 2].y() > 0.01 && ps[i + 2].y() < 10)
                         {
                             ps[i].setY(ps[i - 1].y());
                         }
                     }
                 }
                if (fabs(ps[i].x() + start - wcp[j].x()) < 0.01)
                {
                    ps[i].setY(ps[i].y() + wcp[j].y());
                    j++;
                    break;
                }
                else if (wcp[j].x() < ps[i].x() + start)
                {
                    j++;
                }
                else
                {
                    break;
                }
            }
        }        
        if (!PLScanner::instance()->saveCorrectedData)
        {
            return;
        }

        QString fileName = PLScanner::instance()->correctedDataDir + "/" + QDateTime::currentDateTime().toString("yyyyMMddhhmmss") + ".csv";

        PLScanner::instance()->saveCSV(fileName, ps);
    }

    double PLScannerWorkerImpl::wideCalib(double pos)
    {
        //         auto wideCalib_A = Core::OiSettings()->value("Calib/WideCalib-A", 0.0).toDouble();
        //         auto wideCalib_B = Core::OiSettings()->value("Calib/WideCalib-B", 0.0).toDouble();
        // 
        //         return  wideCalib_A * pos + wideCalib_B;

        if (!PLScanner::instance()->wideCalibEnable)
        {
            return 0.0;
        }

        auto wcp = PLScanner::instance()->wideCalib_P;

        for (int j = 0; j < wcp.size(); j++)
        {
            if (fabs(pos - wcp[j].x()) < 0.01)
            {
                return wcp[j].y();
            }
        }

        return 0.0;
    }

    void PLScannerWorkerImpl::init()
    {
        OI_Q(PLScannerWorker);

        Environment::_startTask = false;
        Environment::_newParams = false;

        q->connect(&_timerProduct, &QTimer::timeout, q, &PLScannerWorker::product);
        q->connect(&_updateChartTimer, &QTimer::timeout, q, &PLScannerWorker::onUpdateChart);
        PLScannerCore::LTCameraDriver::instance();
        _timeCalib =0;
        _workRet = 0;
        _timerProduct.start(1000);
        //_detectMode = Core::OiSettings()->value("DetectMode", 0).toInt();
        
    }

    void PLScannerWorkerImpl::start()
    {
        OI_Q(PLScannerWorker);

        _profileMeasurementTasks.clearQueue();
        _currentProfile.clear();

        LTCameraDriver::instance()->setMaxROI();
        LTCameraDriver::instance()->setCallback(PLScannerWorkerImpl::snapRefresh, q);

        QtConcurrent::run(LTCameraDriver::instance(), &LTCameraDriver::snap);
        _updateChartTimer.start(500);
        OiShowMessage(PLScannerWorker::tr("正在打开相机"));
    }

    void PLScannerWorkerImpl::stop()
    {
        OI_Q(PLScannerWorker);
        LTCameraDriver::instance()->stop();
    }

    void PLScannerWorkerImpl::snapRefresh(const LTCameraFrame& rangeMaps, const QDateTime& timestamp, void* data)
    {
        auto self = static_cast<PLScannerWorker*>(data);
        bool result = QMetaObject::invokeMethod(self, "onSnapRefresh", Qt::QueuedConnection, Q_ARG(const LTCameraFrame&, rangeMaps));
    }

    void PLScannerWorkerImpl::refresh(const PLScannerCore::LTCameraFrame& rangeMaps, const QDateTime& timestamp, void* data)
    {
        auto self = static_cast<PLScannerWorker*>(data);
        QMetaObject::invokeMethod(self, "onRefresh", Qt::QueuedConnection,Q_ARG(const LTCameraFrame&, rangeMaps), Q_ARG(const QDateTime&, timestamp));
    }

    void PLScannerWorkerImpl::exit()
    {
        _timerProduct.stop();

        Environment::_startTask = false;

    }

    PLScannerWorker::PLScannerWorker(QWidget* parent) : QWidget(parent)
    {
        OI_I(PLScannerWorker)->init();
    }

    PLScannerWorker::~PLScannerWorker()
    {
        OI_E(PLScannerWorker)->exit();
    }

    void PLScannerWorker::setParams(const ProductParams& params)
    {
        OI_F(PLScannerWorker);
        QMutexLocker locker(&d->_mutex);

        LTCameraDriver::instance()->flushFormula();

        if (d->_params != params)
        {
            d->_params = params;
            Environment::_newParams = true;
            if (isTempl() && d->_params.templMsrFlag)
            {
                if (d->_measureTool)
                {
                    delete d->_measureTool;
                    d->_measureTool = nullptr;
                }
                d->_measureTool = new MeasureTool(d->_params);
            }

            std::vector<Math::Point2f> paramProfile;
            std::vector<cv::Point2f> paramProfileCV;
            for (int i = 1; i < d->_params.profiles.size(); i++)
            {
                auto arr = d->_params.profiles[i];
                for (auto& p : arr)
                {
                    paramProfile.emplace_back(float(p.x()), float(p.y()));
                    paramProfileCV.emplace_back(float(p.x()), float(p.y()));
                }
            }
            d->_template = paramProfileCV;
            d->_templateTree = Core::KDTree::create(paramProfile);
        }
    }

    int PLScannerWorker::isRunning() const
    {
        OI_C(PLScannerWorker);
        return d->_isRunning;
    }

    WorkResult::Result PLScannerWorker::result()
    {
        OI_F(PLScannerWorker);

        QMutexLocker locker(&d->_mutex);
        return d->_result;
    }

    const std::vector<QPointF>& PLScannerWorker::msrResult() const
    {
        OI_C(PLScannerWorker);
        return d->_curMsrRes;
    }

    double PLScannerWorker::value()
    {
        OI_F(PLScannerWorker);

        QMutexLocker locker(&d->_mutex);
        return d->_fixedValue;
    }

    void PLScannerWorker::start(int mode)
    {
        OI_F(PLScannerWorker);
        return d->start();
    }
     
    void PLScannerWorker::onSnapRefresh(const LTCameraFrame& rangeMaps)
    {
        OI_F(PLScannerWorker);

        QList<QPointF> points;
        LT360X::instance()->rangeMapsToPoints(rangeMaps, points);

        //if (d->_detectMode == 0)
        //{
        //    std::vector<Math::Point2f> fullProfile;
        //    for (auto& point : points)
        //    {
        //        fullProfile.emplace_back(float(point.x()), float(point.y()));
        //    }
        //    d->_firstProfileSize = fullProfile.size();
        //
        //    onMatchDone();
        //}
        //else
        {
            QtConcurrent::run(this, &PLScannerWorker::onMatch, points);
        }
    }

    void PLScannerWorker::onRefresh(const LTCameraFrame& rangemaps, const QDateTime& timestamp)
    {
        OI_F(PLScannerWorker);

        if (rangemaps.contains(LT360X::instance()->cameraNames[0]))
        {
            d->_accumulatedFrames = d->_accumulatedFrames + rangemaps[LT360X::instance()->cameraNames[0]]->count();
        }

        auto currentSin = d->_currentPose.elementAt(1, 0);
        auto currentCos = d->_currentPose.elementAt(0, 0);
        auto currentAngle = std::atan2(currentSin, currentCos) / CV_PI * 180.0;

        auto initialSin = d->_initialPose.elementAt(1, 0);
        auto initialCos = d->_initialPose.elementAt(0, 0);
        auto initialAngle = std::atan2(initialSin, initialCos) / CV_PI * 180.0;

        auto angleDifference = std::fabs(currentAngle - initialAngle);

        if (angleDifference < 45 || angleDifference > 315)
        {
            auto task = new ProfileMeasurementWorker(this, rangemaps, timestamp, d->_template, d->_templateTree, d->_currentPose);
            task->setTempl(isTempl());
            task->setMsrTool(d->_measureTool);
            d->_profileMeasurementTasks.enqueue(task);
        }
        else
        {
            auto task = new ProfileMeasurementWorker(this, rangemaps, timestamp, d->_template, d->_templateTree, d->_initialPose);
            task->setTempl(isTempl());
            task->setMsrTool(d->_measureTool);
            d->_profileMeasurementTasks.enqueue(task);
        }
    }

    void PLScannerWorker::onMatchDone()
    {
        OI_F(PLScannerWorker);
        LTCameraDriver::instance()->setCallback(PLScannerWorkerImpl::refresh, this);
        LTCameraDriver::instance()->start();

        d->_isRunning = true;
        OiShowMessage(PLScannerWorker::tr("正在获取图像"));
        emit enterTask();
    }

    void PLScannerWorker::onReStartCamera()
    {
        OiWarning() << PLScannerWorker::tr("相机不同步，正在重启相机!");
        QtConcurrent::run(LTCameraDriver::instance(), &LTCameraDriver::restartCamera);
    }
    
    void PLScannerWorker::measureDone(
        const LTCameraFrame& rangeMaps,
        const QDateTime& timestamp,
        const QList<QPointF>& profile,
        const std::vector<QPointF>& outputs,
        const Math::Matrix33f& currentPose)
    {
        OI_F(PLScannerWorker);
        d->_currentProfile = profile;
        d->_currentPose = currentPose;
        d->_profileMeasurementTasks.dequeue();
        d->_curMsrRes = outputs;
    }

    void PLScannerWorker::measureDone()
    {
        OI_F(PLScannerWorker);

        d->_profileMeasurementTasks.dequeue();
        d->_currentProfile.clear();
        emit profileEmpty();
    }

    QList<QList<QPointF>> PLScannerWorker::getUpdateProfile(QList<QList<QPointF>> parsedpoints, int leftCcenter, int rightCenter)
    {
        QList<QPointF> points;

        for (int i = 0; i < parsedpoints.size(); i++)
        {
            for (int j = 0; j < parsedpoints[i].size(); j++)
            {
                points.append(parsedpoints[i][j]);
            }
        }
        std::sort(points.begin(), points.end(), [](const QPointF& p1, const QPointF& p2) {return p1.x() < p2.x(); });
        double dmax = 0.0;
        double dxPos;
        //找出左右侧分离点
        for (int i = 0; i < points.size() - 1; i++)
        {
            if (qAbs(points[i].x() - points[i + 1].x()) > dmax)
            {
                dmax = qAbs(points[i].x() - points[i + 1].x());
                dxPos = points[i].x();
            }
        }
        QMap<double, QList<QPointF>> mapPoints;
        QList<QPointF> leftPoints;
        QList<QPointF> rightPoints;
        for (int i = 0; i < parsedpoints.size(); i++)
        {
            for (int j = 0; j < parsedpoints[i].size() - 1; j++)
            {
                if (parsedpoints[i][j].x() - parsedpoints[i][j + 1].x() == 0 && parsedpoints[i][j].y() - parsedpoints[i][j + 1].y() == 0)
                {
                    continue;
                }
                QString strX = QString::number(parsedpoints[i][j].x(), 'f', 1);
                double x1 = strX.toDouble();
                if (x1 <= parsedpoints[i][j].x())
                {
                    x1 += 0.05;
                }

                QPointF p(x1, parsedpoints[i][j].y());
                {
                    if (!mapPoints.contains(x1))
                    {
                        QList<QPointF> tmpPoints;
                        tmpPoints.append(p);
                        mapPoints.insert(x1, tmpPoints);
                    }
                    else
                    {
                        mapPoints[x1].append(p);
                    }
                }
            }
        }
        QList<double> keyList = mapPoints.keys();
#pragma omp parallel for 
        for (int i = 0; i < keyList.size(); i++)
        {
            QList<QPointF> lstPoints = mapPoints[keyList[i]];
            double dpos = keyList[i];

            for (double k = 0; k < 0.5; k += 0.05)
            {

                if (keyList.contains(dpos - k))
                {
                    lstPoints.append(mapPoints[dpos - k]);
                }
                if (keyList.contains(dpos + k))
                {
                    lstPoints.append(mapPoints[dpos + k]);
                }
            }
            if (lstPoints.size() < 1)
            {
                continue;
            }
            std::sort(lstPoints.begin(), lstPoints.end(), [](const QPointF& p1, const QPointF& p2) {return p1.y() < p2.y(); });
            QPointF dPoint = lstPoints[0];
            QPointF uPoint = lstPoints[lstPoints.size() - 1];
            if (keyList[i] > dxPos + 1)
            {
#pragma omp critical
                {
                    rightPoints.append(QPointF(keyList[i], (uPoint.y() - dPoint.y())/ PLScanner::instance()->scaleView));
                }               
            }
            else
            {
#pragma omp critical
                {
                    leftPoints.append(QPointF(keyList[i], (uPoint.y() - dPoint.y()) / PLScanner::instance()->scaleView));
                }
            }
        }
        std::sort(leftPoints.begin(), leftPoints.end(), [](const QPointF& p1, const QPointF& p2) {return p1.x() < p2.x(); });
        std::sort(rightPoints.begin(), rightPoints.end(), [](const QPointF& p1, const QPointF& p2) {return p1.x() < p2.x(); });
        double dmaxVal = 0.0;
        for (int i = 1; i < leftPoints.size() - 1; i++)
        {
            double a = qAbs(leftPoints[i].y() - leftPoints[i - 1].y());
            if (qAbs(leftPoints[i].y() - leftPoints[i - 1].y()) > 0.5/ PLScanner::instance()->scaleView || qAbs(leftPoints[i].y() - leftPoints[i + 1].y()) > 0.5/PLScanner::instance()->scaleView)
            {
                leftPoints.removeAt(i);
                i--;
                continue;
            }
            if (qAbs(leftPoints[i].y() - leftPoints[i - 1].y()) > dmaxVal)
            {
                dmaxVal = qAbs(leftPoints[i].y() - leftPoints[i - 1].y());
            }
            if (qAbs(leftPoints[i].y() - leftPoints[i + 1].y()) > dmaxVal)
            {
                dmaxVal = qAbs(leftPoints[i].y() - leftPoints[i + 1].y());
            }
        }
        for (int i = 1; i < rightPoints.size() - 1; i++)
        {

            if (qAbs(rightPoints[i].y() - rightPoints[i - 1].y()) > 0.5 / PLScanner::instance()->scaleView || qAbs(rightPoints[i].y() - rightPoints[i + 1].y()) > 0.5 / PLScanner::instance()->scaleView)
            {
                rightPoints.removeAt(i);
                i--;
            }
        }

        QList<QList<QPointF>> profilePoints;
        profilePoints.append(leftPoints);
        profilePoints.append(rightPoints);
        return profilePoints;
       
    }

    bool PLScannerWorker::isTempl() const
    {
        OI_C(PLScannerWorker);
        auto& dfs = d->_params.definitionPoints;
        if (!dfs.isEmpty())
        {
            DefinitionType tp = static_cast<DefinitionType>(dfs[0].type);
            return tp >= DT_TempleStandLine && tp <= DT_TempleStandArcsMark;
        }
        return false;
    }

    void PLScannerWorker::onUpdateChart()
    {
        OI_F(PLScannerWorker);

        if (d->_currentProfile.size() < 50)
        {
            return;
        }

        //if (d->_detectMode == 0)
        //{
        //    QList<QPointF> points;
        //    auto parsedProfile = Math::profileParser(
        //        d->_currentProfile,
        //        LT360X::instance()->connectionThreshold(),
        //        LT360X::instance()->linkingThreshold(),
        //        LT360X::instance()->noiseThreshold());
        //    QList<QList<QPointF>> parsedPoints = getUpdateProfile(parsedProfile, 0, 0);
        //
        //    QList<QPointF> leftProfile = parsedPoints[0];
        //    QList<QPointF> rightProfile = parsedPoints[1];
        //
        //    WorkResult::Result mResult;
        //    mResult.profile1 = leftProfile;
        //    mResult.profile2 = rightProfile;
        //
        //    d->_mutex.lock();
        //    mResult.params = d->_params;
        //    d->_mutex.unlock();
        //
        //    WorkResult::Result::caculateResult(mResult, true, NULL);
        //
        //    d->_mutex.lock();
        //    d->_result = mResult;
        //    d->_mutex.unlock();
        //    emit leaveRound();
        //}
        //else
        {
            WorkResult::Result mResult;
            mResult.result = WorkResult::RC_Success;

            mResult.profile1 = d->_currentProfile;
            mResult.params = d->_params;
            //WorkResult::Result::caculateResult(mResult, true, NULL);
            d->_mutex.lock();
            d->_result = mResult;
            d->_mutex.unlock();
            emit leaveRound();
        }
    }

    void PLScannerWorker::stop()
    {
        OI_F(PLScannerWorker);

        d->_workRet = 1;        
        d->_isRunning = false;
        d->_profileMeasurementTasks.clearQueue();
        d->_updateChartTimer.stop();
        d->_currentProfile.clear();
        LTCameraDriver::instance()->stop();
        emit leaveTask();
    }

    void PLScannerWorker::onLeaveTask()
    {
        OI_F(PLScannerWorker);

        Motor::instance()->locateMode();
        Motor::instance()->locatedRun(0.0);

        Motor::instance()->setResult(10000);

        SensorAcquire::OiAcquire()->abort();

        Environment::_startTask = false;
    }

    void PLScannerWorker::product()
    {
        OI_F(PLScannerWorker);

        auto outputSystem = Core::OiSettings()->value("System/OutputSystem").toString();
        if (!outputSystem.isEmpty())
        {
            if (PLScannerOutput::heartbeat(d->_heartbeat, outputSystem))
            {

            }

            if (d->_heartbeat == 0)
            {
                d->_heartbeat = 1;
            }
            else
            {
                d->_heartbeat = 0;
            }
        }

        if (PLScanner::instance()->paramMode == 0)
        {
            return;
        }

        auto productSystem = Core::OiSettings()->value("System/ProductSystem").toString();
        if (productSystem.isEmpty())
        {
            return;
        }
        if (PLScanner::instance()->paramMode == 1)
        {
            ProductParams product;

            if (ProductParams::loadFromSys(product, productSystem))
            {

                //QMutexLocker locker(&d->_mutex);

                if (d->_params != product)
                {
                    ProductParams::saveToLocal(product);
                    ProductParams::setNext(product.specName);
                    ProductParams::updateParams();

                }
            }
        }
        else if (PLScanner::instance()->paramMode == 2)
        {
            QString specName;

            if (ProductParams::loadNameFromSys(specName, productSystem))
            {
                if (Motor::instance()->switchEnable())
                {
                    if (d->_params.specName != specName && ProductParams::next() != specName)
                    {
                            ProductParams::setNext(specName);
                            ProductParams::updateParams();   
                    }
                }
                else
                {
                    OiWarning() << "Motor::instance()->switchEnable = False"<<endl;
                }
            }
        }
    }

    void PLScannerWorker::save(const QString& path)
    {
        OI_F(PLScannerWorker);

        d->_mutex.lock();
        WorkResult::Result::saveResultToFile(d->_result, path);
        d->_mutex.unlock();
    }

    void PLScannerWorker::open(const QString& file)
    {
        OI_F(PLScannerWorker);

        OiProfile profile;

        if (!SensorAcquire::OiAcquire()->acquire(file, profile))
        {
            return;
        }

        PLScannerUtil::filter(profile, PLScanner::instance()->sideCorrect, 0.5, PLScanner::instance()->filter, PLScanner::instance()->filterLen);

        if (profile.size() < 20)
        {
            OiAlerter() << tr("轮廓数不足");
            return;
        }

        d->wideCalib(profile);

        WorkResult::Result mResult;
        mResult.profile1 = profile;
        mResult.params = d->_params;

        WorkResult::Result::caculateResult(mResult, true, NULL);

        d->_mutex.lock();
        d->_result = mResult;
        d->_mutex.unlock();
        emit leaveRound();

    }

    void PLScannerWorker::onMatch(const QList<QPointF>& points)
    {
        OI_F(PLScannerWorker);

        if (Core::OiSettings()->value("testMode", false).value<bool>())
        {
            d->_firstProfileSize = d->_template.size();
            d->_currentPose = Math::Matrix33f::identity();
        }
        else
        {
            d->_firstProfileSize = points.size();

            if (points.size() < 50)
            {
                OiAlerter() << "轮廓缺失!";

                QMetaObject::invokeMethod(this, "alerterStop");

                return;
            }

            d->_currentPose = accurateMatch(points, d->_templateTree);
        }
        
        d->_initialPose = d->_currentPose;

        QMetaObject::invokeMethod(this, "onMatchDone");
    }

} }
